#include <iostream>
#include <vector>
#include <iomanip>
#include "serial_csv.h"

SerialWrapper::SerialWrapper(const std::string &port, unsigned int baudrate, boost::asio::serial_port_base::character_size bytesize,
                             boost::asio::serial_port_base::parity parity, boost::asio::serial_port_base::stop_bits stopbits)
    : port(port), baudrate(baudrate), bytesize(bytesize), parity(parity), stopbits(stopbits), serial(io) {}

bool SerialWrapper::open_serial()
{
    try
    {
        serial.open(port);
        serial.set_option(boost::asio::serial_port_base::baud_rate(baudrate));
        serial.set_option(bytesize);
        serial.set_option(parity);
        serial.set_option(stopbits);
        std::cout << "Serial port configured: BaudRate = " << baudrate
                  << ", StopBits = " << stop_bits_to_string(stopbits)
                  << ", Parity = " << parity_to_string(parity) << std::endl;
        return true;
    }
    catch (boost::system::system_error &e)
    {
        std::cerr << "Error opening serial port: " << e.what() << std::endl;
        return false;
    }
}

void SerialWrapper::close()
{
    if (serial.is_open())
    {
        serial.close();
        std::cout << "Serial port closed" << std::endl;
    }
}

void SerialWrapper::write_data(const std::vector<uint8_t> &data)
{
    while (running)
    {
        try
        {
            boost::asio::write(serial, boost::asio::buffer(data));
            std::this_thread::sleep_for(std::chrono::milliseconds(measure_interval)); // Simulate interval sending
        }
        catch (boost::system::system_error &e)
        {
            if (!running)
                break;
            std::cerr << "Error writing to port: " << e.what() << std::endl;
        }
    }
}

void SerialWrapper::read_data()
{
    while (running)
    {
        try
        {
            uint8_t buf[256];
            boost::system::error_code ec;
            size_t len = serial.read_some(boost::asio::buffer(buf), ec);
            if (ec)
            {
                if (!running)
                    break;
                std::cerr << "Error reading from port: " << ec.message() << std::endl;
            }
            else if (len > 0)
            {
                process_received_data(buf, len);
            }
        }
        catch (boost::system::system_error &e)
        {
            if (!running)
                break;
            std::cerr << "Error reading from port: " << e.what() << std::endl;
        }
    }
}

void SerialWrapper::process_received_data(uint8_t *data, size_t length)
{
    std::lock_guard<std::mutex> lock(data_mutex);
    data_queue.insert(data_queue.end(), data, data + length);

    while (data_queue.size() >= 7)
    {
        if (data_queue[0] == 0x01 && data_queue[1] == 0x03 && data_queue[2] == 0x02)
        {
            std::vector<uint8_t> valid_data(data_queue.begin(), data_queue.begin() + 7);

            int combined_value = (valid_data[3] << 8) | valid_data[4];
            std::cout << "Measure Distance is: " << std::dec << combined_value << " cm" << std::endl;

            last_measurement = current_measurement;
            current_measurement = combined_value;

            last_time = current_time;
            current_time = std::chrono::steady_clock::now();

            data_queue.erase(data_queue.begin(), data_queue.begin() + 7);
        }
        else
        {
            data_queue.erase(data_queue.begin());
        }
    }
}

void SerialWrapper::calculate_speed()
{
    while (running)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(measure_interval));

        std::lock_guard<std::mutex> lock(data_mutex);
        if (last_measurement != 0 && current_measurement != 0)
        {
            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_time).count();
            if (duration > 0)
            {
                double distance_diff = current_measurement - last_measurement;
                double time_diff = duration / 1000.0; // convert to seconds
                double speed = distance_diff / time_diff; // cm/s
                std::cout << "Current Measurement: " << std::abs(current_measurement) << " cm" << std::endl;
                std::cout << "Last Measurement: " << std::abs(last_measurement) << " cm" << std::endl;
                std::cout << "Time Difference: " << std::abs(time_diff) << " s" << std::endl;
                if (speed < 0)
                {
                    std::cout << "Speed of approaching: " << std::abs(speed) << " cm/s" << std::endl;
                }
                else
                {
                    std::cout << "Speed of moving away: " << std::abs(speed) << " cm/s" << std::endl;
                }
                save_to_csv(current_measurement, time_diff, speed);
            }
        }
    }
}

void SerialWrapper::save_to_csv(int measurement, double time_diff, double speed)
{
    auto now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
    std::ofstream file("data.csv", std::ios::app);
    if (file.is_open())
    {
        file << std::put_time(std::localtime(&now), "%Y-%m-%d %H:%M:%S") << ","
             << measurement << "," << time_diff << "," << speed << "\n";
    }
    else
    {
        std::cerr << "Error opening file to save data" << std::endl;
    }
}

void SerialWrapper::ensure_file_exists(const char* file_name)
{
    std::ifstream infile(file_name);
    if (!infile.good())
    {
        std::ofstream file(file_name, std::ios::app);
        if (file.is_open())
        {
            file << "Timestamp,Distance (cm),Time Diff (s),Speed (cm/s)\n";
        }
    }
}

std::string SerialWrapper::stop_bits_to_string(boost::asio::serial_port_base::stop_bits stopbits)
{
    switch (stopbits.value())
    {
    case boost::asio::serial_port_base::stop_bits::one:
        return "1";
    case boost::asio::serial_port_base::stop_bits::onepointfive:
        return "1.5";
    case boost::asio::serial_port_base::stop_bits::two:
        return "2";
    default:
        return "unknown";
    }
}

std::string SerialWrapper::parity_to_string(boost::asio::serial_port_base::parity parity)
{
    switch (parity.value())
    {
    case boost::asio::serial_port_base::parity::none:
        return "none";
    case boost::asio::serial_port_base::parity::odd:
        return "odd";
    case boost::asio::serial_port_base::parity::even:
        return "even";
    default:
        return "unknown";
    }
}

int main()
{
    std::cout << " +----------------------------------+" << std::endl;
    std::cout << " |        Serial Port Write         |" << std::endl;
    std::cout << " +----------------------------------+" << std::endl;

    std::vector<uint8_t> write_buffer = {0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0A};

    SerialWrapper serial_wrapper("/dev/ttyUSB0", 115200, boost::asio::serial_port_base::character_size(8),
                                 boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none),
                                 boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));

    serial_wrapper.ensure_file_exists("data.csv");  // Ensure data.csv exists

    if (serial_wrapper.open_serial())
    {
        std::thread send_thread(&SerialWrapper::write_data, &serial_wrapper, std::ref(write_buffer));
        std::thread receive_thread(&SerialWrapper::read_data, &serial_wrapper);
        std::thread speed_thread(&SerialWrapper::calculate_speed, &serial_wrapper);

        send_thread.join();    // Wait for the send thread to finish
        receive_thread.join(); // Wait for the receive thread to finish
        speed_thread.join();   // Wait for the speed calculation thread to finish
    }

    serial_wrapper.close();
    std::cout << "\nSerial communication ended" << std::endl;
    return 0;
}
